/*
 * @Descripttion: 
 * @version: V1.0.0
 * @Author: LeonHe
 * @Date: 2021-05-28 12:12:57
 * @LastEditTime: 2021-05-30 15:57:21
 */
#include "PidContorller.h"

void PidControllerQ15Func(pidConQ15StDef* inst)
{
    #if 1
    //误差计算
    inst->param.state[2] = inst->param.state[1];
    inst->param.state[1] = inst->param.state[0];
    inst->param.state[0] = inst->ref - inst->fbk;

    //PID计算
    inst->out += ((q31_t)inst->param.A0*inst->param.state[0] + 
                    (q31_t)inst->param.A1*inst->param.state[1] +
                    (q31_t)inst->param.A2*inst->param.state[2])/0x8000;
    #else
#if 0
q63_t acc;
q15_t out;

q15_t temp[2];

    /* acc = A0 * x[n]  */
    //acc = ((q31_t) inst->param.A0) * (inst->ref - inst->fbk);
    temp[0] = (((q31_t) inst->param.A0) * (inst->ref - inst->fbk))>>15;
    /* acc += A1 * x[n-1] + A2 * x[n-2]  */
    //acc += (q31_t) inst->param.A1 * inst->param.state[0];
    //acc += (q31_t) inst->param.A2 * inst->param.state[1];
    temp[1] = (((q31_t) inst->param.A1) * (inst->param.state[0]))>>15;
    temp[1]++;
    
    // SEGGER_RTT_printf(0,"1:%d\r\n", -temp[1]);
    // SEGGER_RTT_printf(0,"2:%d\r\n", temp[0]);
    acc = temp[0] + temp[1];

    /* acc += y[n-1] */
    // acc += (q31_t) inst->param.state[2] << 15;
    out = inst->param.state[2] + temp[0] + temp[1];
    

    /* saturate the output */
    //out = (q15_t) (__SSAT((acc >> 15), 16));

    /* Update state */
    inst->param.state[1] = inst->param.state[0];
    inst->param.state[0] = inst->ref - inst->fbk;
    inst->param.state[2] = out;

    /* return to application */
    inst->out = out;
#endif



    #endif
}

void PidControllerQ31Func(pidConQ31StDef* inst)
{
    //误差计算
    inst->param.state[2] = inst->param.state[1];
    inst->param.state[1] = inst->param.state[0];
    inst->param.state[0] = inst->ref - inst->fbk;

    //PID计算
    inst->out += ((q63_t)inst->param.A0*inst->param.state[0] + 
                    (q63_t)inst->param.A1*inst->param.state[1] +
                    (q63_t)inst->param.A2*inst->param.state[2])/0x80000000;
}

